import asyncio
import json
import time
import serial
import os
import signal
import sys
from websockets import connect, exceptions
from smbus2 import SMBus
from configparser import ConfigParser
from bus_driver.bus_driver import BusDriver  # 导入总线舵机驱动

# -------------------------- 核心配置（根据舵机型号调整）

CONFIG_FILE = "/www/wwwroot/new-human-control/pi/config.ini"
PCA9685_ADDR = 0x40  # 默认I2C地址
PWM_FREQ = 50  # 舵机标准频率（50Hz，周期20ms）
BUS_NUMBER = 1  # I2C总线号（树莓派通常为1）
MIN_PWM = 110  # 最小PWM值（避免堵转）
MAX_PWM = 520  # 最大PWM值（根据舵机手册或测试结果修改）

# 串口配置
SERIAL_PORT = '/dev/ttyAMA3'  # 串口端口
SERIAL_BAUDRATE = 115200  # 串口波特率
SERIAL_TIMEOUT = 1  # 串口读取超时时间（秒）

# 总线舵机配置
BUS_SERVO_PORT = '/dev/ttyAMA2'  # 总线舵机串口端口
BUS_SERVO_BAUDRATE = 115200  # 总线舵机波特率

# 全局变量
board_name = "body"
wifi_configured = False
running = True
controlled_channels = set()  # 记录已控制的舵机通道（用于退出时复位）
config = ConfigParser()  # 初始化配置解析器
serial_ser = None  # 串口实例全局变量
bus_driver = None  # 总线舵机驱动实例


# ---------------------------------- 总线舵机初始化-------------------------------------
def init_bus_servo():
    """初始化总线舵机驱动"""
    global bus_driver
    try:
        bus_driver = BusDriver(port=BUS_SERVO_PORT, baudrate=BUS_SERVO_BAUDRATE)
        if bus_driver._init_serial():
            print("总线舵机驱动初始化成功")
            # 扫描总线上的舵机
            found_servos = bus_driver.scan_servos()
            if found_servos:
                print(f"发现总线舵机: {found_servos}")
            else:
                print("未发现总线舵机，请检查连接")
            return True
        else:
            print("总线舵机驱动初始化失败")
            return False
    except Exception as e:
        print(f"总线舵机初始化异常: {e}")
        return False


# -------------------------- 异步串口数据读取函数 --------------------------
async def async_serial_data_reader():
    """
    异步串口数据读取函数（适配asyncio事件循环）
    实时读取串口数据，并在main所在的主循环中打印
    """
    global serial_ser
    print("=== 异步串口读取功能初始化 ===")

    # 初始化串口
    try:
        serial_ser = serial.Serial(
            port=SERIAL_PORT,
            baudrate=SERIAL_BAUDRATE,
            timeout=SERIAL_TIMEOUT
        )
        if not serial_ser.is_open:
            serial_ser.open()
        print(f"串口已打开：{SERIAL_PORT}（波特率：{SERIAL_BAUDRATE}）")
    except Exception as e:
        print(f"串口初始化失败：{e}")
        return

    # 异步持续读取（用asyncio.sleep避免阻塞事件循环）
    try:
        while running:
            if serial_ser.in_waiting > 0:
                try:
                    # 读取并解码数据（与原逻辑一致，读取后直接打印）
                    recv_data = serial_ser.read(serial_ser.in_waiting).decode('utf-8', errors='ignore').strip()
                    if recv_data:
                        # 此处打印即为主函数所在主循环的输出（因共享事件循环）
                        print(f"[main-串口接收] {recv_data}")
                except UnicodeDecodeError:
                    print(f"[main-串口接收] 数据解码失败（非UTF-8格式）")
            # 异步休眠（0.1秒，降低CPU占用，且不阻塞其他任务）
            await asyncio.sleep(0.1)
    except Exception as e:
        print(f"[main-串口] 读取异常：{e}")
    finally:
        # 关闭串口（退出时执行）
        if serial_ser and serial_ser.is_open:
            serial_ser.close()
            print("[main-串口] 串口已关闭")
        print("=== 异步串口读取功能停止 ===")


# -------------------------- PCA9685 驱动类（适配舵机） --------------------------
class PCA9685:
    def __init__(self, address=PCA9685_ADDR, bus_number=BUS_NUMBER):
        self.address = address
        self.bus = SMBus(bus_number)
        self._init_device()

    def _init_device(self):
        """设备完整初始化（包含复位、频率配置、推挽输出）"""
        # 验证设备连接
        try:
            self.bus.read_byte_data(self.address, 0x00)
            print(f"PCA9685 设备检测成功（地址: 0x{self.address:02X}）")
        except OSError as e:
            raise OSError(f"无法连接PCA9685（地址: 0x{self.address:02X}）: {e}")

        # 软件复位
        self.software_reset()
        # 设置PWM频率
        self.set_pwm_freq(PWM_FREQ)
        # 配置推挽输出
        self.bus.write_byte_data(self.address, 0x01, 0x04)
        print("PCA9685 已配置推挽输出模式（适配舵机驱动）")

    def software_reset(self):
        """软件复位"""
        self.bus.write_byte_data(self.address, 0x00, 0x00)
        time.sleep(0.1)
        print("PCA9685 软件复位完成")

    def set_pwm_freq(self, freq):
        """设置PWM频率"""
        prescaleval = 25000000.0 / 4096.0 / float(freq) - 1.0
        prescale = int(round(prescaleval))

        old_mode = self.bus.read_byte_data(self.address, 0x00)
        new_mode = (old_mode & 0x7F) | 0x10
        self.bus.write_byte_data(self.address, 0x00, new_mode)
        self.bus.write_byte_data(self.address, 0xFE, prescale)
        self.bus.write_byte_data(self.address, 0x00, old_mode)
        time.sleep(0.005)
        self.bus.write_byte_data(self.address, 0x00, old_mode | 0x80)
        print(f"PCA9685 PWM频率设置为 {freq}Hz（预分频值: {prescale}）")

    def set_pwm(self, channel, off, on=0):
        """设置PWM值（ON固定为0，OFF为有效脉冲值）"""
        if not (0 <= channel <= 15):
            raise ValueError(f"通道必须在0-15之间（当前: {channel}）")
        off = max(MIN_PWM, min(MAX_PWM, off))

        base_reg = 0x06 + 4 * channel
        self.bus.write_byte_data(self.address, base_reg, on & 0xFF)
        self.bus.write_byte_data(self.address, base_reg + 1, on >> 8)
        self.bus.write_byte_data(self.address, base_reg + 2, off & 0xFF)
        self.bus.write_byte_data(self.address, base_reg + 3, off >> 8)

        controlled_channels.add(channel)

    def set_servo_angle(self, channel, angle):
        """设置舵机角度（0-180°）"""
        if not (0 <= angle <= 180):
            raise ValueError(f"角度必须在0-180之间（当前: {angle}）")

        pulse = int(MIN_PWM + (angle / 180.0) * (MAX_PWM - MIN_PWM))
        self.set_pwm(channel, off=pulse)
        print(f"舵机通道{channel}: 角度{angle}° → PWM值{pulse}")


# -------------------------- 初始化PCA9685 --------------------------
try:
    pwm = PCA9685()
except Exception as e:
    print(f"PCA9685初始化失败，程序退出: {e}")
    sys.exit(1)


# -------------------------- 舵机控制逻辑（修改：集成总线舵机） --------------------------
def handle_servo_control(obj):
    """处理舵机控制命令（支持PCA9685和总线舵机）"""

    # 解析通道/ID
    channel_keys = ["c", "channel", "ch", "C", "Channel"]
    id_keys = ["id", "servo_id", "sid", "ID", "ServoID"]

    channel_or_id = None

    # 首先尝试解析为通道（PCA9685）
    for key in channel_keys:
        if key in obj:
            channel_or_id = obj[key]
            break

    # 如果没有找到通道，尝试解析为ID（总线舵机）
    if channel_or_id is None:
        for key in id_keys:
            if key in obj:
                channel_or_id = obj[key]
                break

    if channel_or_id is None:
        print(f"舵机控制缺少通道/ID参数（支持通道: {channel_keys} / ID: {id_keys}）: {obj}")
        return

    try:
        channel_or_id = int(channel_or_id)
    except ValueError:
        print(f"舵机通道/ID必须为整数（当前: {channel_or_id}）")
        return

    # 判断是PCA9685舵机还是总线舵机
    # 假设：通道0-15为PCA9685，ID 1-254为总线舵机
    is_bus_servo = (channel_or_id >= 1 and channel_or_id <= 254)

    if is_bus_servo and bus_driver:
        # 总线舵机控制
        servo_id = channel_or_id

        # 解析角度控制
        angle_keys = ["angle", "a", "Angle", "ANGLE"]
        for key in angle_keys:
            if key in obj:
                try:
                    angle = int(obj[key])
                    speed = obj.get("speed", obj.get("s", 100))  # 默认速度100
                    try:
                        speed = int(speed)
                    except:
                        speed = 100

                    if bus_driver.set_servo_angle(servo_id, angle, speed):
                        print(f"总线舵机 ID{servo_id}: 角度{angle}° (速度{speed})")
                    else:
                        print(f"总线舵机 ID{servo_id} 角度设置失败")
                    return
                except ValueError as e:
                    print(f"总线舵机角度参数错误: {e}（当前: {obj[key]}）")
                    return

        # 解析脉宽控制
        pulse_keys = ["p", "pulse", "pwm", "value", "P", "Pulse"]
        for key in pulse_keys:
            if key in obj:
                try:
                    pulse = int(obj[key])
                    if bus_driver.set_servo_pulse(servo_id, pulse):
                        print(f"总线舵机 ID{servo_id}: 脉宽{pulse}μs")
                    else:
                        print(f"总线舵机 ID{servo_id} 脉宽设置失败")
                    return
                except ValueError as e:
                    print(f"总线舵机脉宽参数错误: {e}（当前: {obj[key]}）")
                    return

        # 解析读取命令
        read_keys = ["read", "r", "get", "Read"]
        for key in read_keys:
            if key in obj:
                angle = bus_driver.read_servo_angle(servo_id)
                if angle is not None:
                    print(f"总线舵机 ID{servo_id} 当前角度: {angle:.1f}°")
                else:
                    print(f"读取总线舵机 ID{servo_id} 角度失败")
                return

        # 无有效参数
        print(f"总线舵机命令缺少有效参数（支持角度: {angle_keys} / 脉宽: {pulse_keys} / 读取: {read_keys}）: {obj}")

    else:
        # PCA9685舵机控制
        channel = channel_or_id

        # 解析脉宽
        pulse_keys = ["p", "pulse", "pwm", "value", "P", "Pulse"]
        for key in pulse_keys:
            if key in obj:
                try:
                    pulse = int(obj[key])
                    pwm.set_pwm(channel, off=pulse)
                    return
                except ValueError as e:
                    print(f"舵机脉宽参数错误: {e}（当前: {obj[key]}）")
                    return

        # 解析角度
        angle_keys = ["angle", "a", "Angle", "ANGLE"]
        for key in angle_keys:
            if key in obj:
                try:
                    angle = int(obj[key])
                    pwm.set_servo_angle(channel, angle)
                    return
                except ValueError as e:
                    print(f"舵机角度参数错误: {e}（当前: {obj[key]}）")
                    return

        # 无有效参数
        print(f"舵机命令缺少有效参数（支持脉宽: {pulse_keys} / 角度: {angle_keys}）: {obj}")


def process_potential_commands(content):
    """递归处理所有可能包含舵机命令的内容"""
    if isinstance(content, dict):
        handle_servo_control(content)
        for value in content.values():
            if isinstance(value, (dict, list)):
                process_potential_commands(value)
    elif isinstance(content, list):
        print(f"处理列表命令（{len(content)}个元素）")
        for item in content:
            process_potential_commands(item)
    elif isinstance(content, str):
        try:
            parsed = json.loads(content)
            print(f"解析字符串命令为JSON: {parsed}")
            process_potential_commands(parsed)
        except json.JSONDecodeError:
            pass


# -------------------------- WebSocket消息处理 --------------------------
def handle_message(message):
    """处理WebSocket接收的消息"""
    try:
        print(f"\n[WebSocket] 收到原始消息: {message}")
        data = json.loads(message)
    except json.JSONDecodeError as e:
        print(f"[WebSocket] JSON解析错误: {e}")
        return None
    except Exception as e:
        print(f"[WebSocket] 消息处理异常: {e}")
        return None

    # 提取消息类型
    msg_type = data.get("type") or data.get("Type") or data.get("TYPE")
    if not msg_type:
        print("[WebSocket] 消息缺少type字段，尝试解析命令")
        process_potential_commands(data)
        return None

    # 处理各类消息
    msg_type = msg_type.lower()
    if msg_type == "heartbeat":
        response = json.dumps({
            "type": "heartbeat",
            "status": "online",
            "name": board_name,
            "timestamp": int(time.time())
        })
        print(f"[WebSocket] 发送心跳响应: {response}")
        return response
    elif msg_type == "register_ack":
        print(f"[WebSocket] 设备注册成功: {data.get('content', '无信息')}")
    elif msg_type == "broadcast":
        content = data.get("content")
        print(f"[WebSocket] 收到广播消息: {content}")
        process_potential_commands(content)
    elif msg_type == "private":
        print(f"[WebSocket] 收到私有消息")
        process_potential_commands(data.get("content", data))
    elif msg_type == "servo_control":
        print(f"[WebSocket] 收到直接舵机控制消息")
        process_potential_commands(data.get("content", data))
    else:
        print(f"[WebSocket] 未知消息类型: {msg_type}，尝试解析命令")
        process_potential_commands(data)

    return None


async def websocket_client():
    """WebSocket客户端主逻辑"""
    global running
    last_heartbeat = 0
    reconnect_interval = 5
    ws_url = "ws://localhost:9102/ws"
    # ws_url = "ws://192.168.31.35:9102/ws"

    while running:
        try:
            async with connect(ws_url, ping_interval=10, ping_timeout=5) as websocket:
                print(f"\n[WebSocket] 成功连接到服务器: {ws_url}")

                # 发送注册消息（更新支持的命令）
                register_msg = json.dumps({
                    "type": "register",
                    "name": board_name,
                    "device_type": "servo_controller",
                    "supported_commands": {
                        "servo": [
                            f"{{c:通道(0-15), p:脉宽({MIN_PWM}-{MAX_PWM})}}",
                            "{{channel:通道(0-15), angle:角度(0-180)}}",
                            "{{id:舵机ID(1-254), angle:角度(0-180), speed:速度(1-255)}}",  # 总线舵机
                            "{{servo_id:舵机ID, pulse:脉宽(500-2500)}}"
                        ]
                    }
                })
                await websocket.send(register_msg)
                print(f"[WebSocket] 发送注册消息: {register_msg}")

                # 循环接收消息
                while running:
                    try:
                        message = await asyncio.wait_for(websocket.recv(), timeout=1.0)
                        response = handle_message(message)
                        if response:
                            await websocket.send(response)

                    except asyncio.TimeoutError:
                        # 发送心跳
                        if time.time() - last_heartbeat > 15:
                            heartbeat_msg = json.dumps({
                                "type": "heartbeat",
                                "status": "online",
                                "timestamp": int(time.time())
                            })
                            await websocket.send(heartbeat_msg)
                            last_heartbeat = time.time()
                            print(f"[WebSocket] 发送心跳: {heartbeat_msg}")

                    except exceptions.ConnectionClosed as e:
                        print(f"[WebSocket] 连接被关闭: {e}，{reconnect_interval}秒后重连")
                        break

        except Exception as e:
            print(f"[WebSocket] 连接失败: {e}，{reconnect_interval}秒后重试")
            await asyncio.sleep(reconnect_interval)


# -------------------------- 配置加载与保存 --------------------------
def load_config():
    """加载配置文件"""
    global board_name, wifi_configured
    if os.path.exists(CONFIG_FILE):
        config.read(CONFIG_FILE)
        if "General" in config:
            board_name = config["General"].get("name", "esp32")
        wifi_configured = True
        print(f"加载配置文件成功（设备名: {board_name}）")
        return True
    return False


def save_config():
    """保存配置文件"""
    config_dir = os.path.dirname(CONFIG_FILE)
    if not os.path.exists(config_dir):
        os.makedirs(config_dir)
    if "General" not in config:
        config["General"] = {}
    config["General"]["name"] = board_name
    with open(CONFIG_FILE, "w") as f:
        config.write(f)
    print(f"保存默认配置文件到: {CONFIG_FILE}")


# -------------------------- 优雅退出（舵机复位+串口关闭） --------------------------
def reset_servos():
    """复位所有已控制的舵机到中间位置"""
    # 复位PCA9685舵机
    if controlled_channels:
        mid_angle = 90
        mid_pulse = int(MIN_PWM + (mid_angle / 180.0) * (MAX_PWM - MIN_PWM))
        print(f"\n开始复位PCA9685舵机（中间位置: 角度{mid_angle}° / PWM{mid_pulse}）")
        for ch in controlled_channels:
            try:
                pwm.set_pwm(ch, off=mid_pulse)
                time.sleep(0.1)
            except Exception as e:
                print(f"舵机通道{ch}复位失败: {e}")
        print("PCA9685舵机复位完成")

    # 复位总线舵机（如果存在）
    if bus_driver and hasattr(bus_driver, 'connected_servos') and bus_driver.connected_servos:
        print(f"\n开始复位总线舵机（中间位置: 角度90°）")
        bus_driver.reset_servos()
        print("总线舵机复位完成")


def signal_handler(sig, frame):
    """信号处理（Ctrl+C/终止信号）"""
    global running
    print(f"\n收到退出信号（{signal.Signals(sig).name}）")
    running = False
    # 复位舵机
    reset_servos()
    # 关闭串口（若存在）
    if serial_ser and serial_ser.is_open:
        serial_ser.close()
        print("普通串口已关闭")
    # 关闭总线舵机串口
    if bus_driver:
        bus_driver.close()
        print("总线舵机串口已关闭")
    print("程序已优雅退出")
    sys.exit(0)


# -------------------------- 主函数 --------------------------
def main():
    """程序入口（整合异步WebSocket和串口任务）"""
    global running

    # 1. 注册信号处理（保持原逻辑）
    signal.signal(signal.SIGINT, signal_handler)
    signal.signal(signal.SIGTERM, signal_handler)

    # 2. 加载配置（保持原逻辑）
    if not load_config():
        print("未找到配置文件，使用默认配置")
        save_config()

    # 3. 初始化总线舵机
    init_bus_servo()

    # 4. 初始化事件循环，添加"WebSocket"和"串口读取"两个异步任务
    print(f"\n舵机控制程序启动完成（设备名: {board_name}）")
    print(f"支持舵机范围：")
    print(f"  - PCA9685舵机：通道0-15 | 角度0-180° | PWM{MIN_PWM}-{MAX_PWM}")
    print(f"  - 总线舵机：ID 1-254 | 角度0-180° | 脉宽500-2500μs")

    try:
        # 获取当前事件循环
        loop = asyncio.get_event_loop()
        # 并行运行两个异步任务（WebSocket客户端 + 异步串口读取）
        loop.run_until_complete(
            asyncio.gather(
                websocket_client(),  # 原WebSocket任务
                async_serial_data_reader()  # 异步串口任务
            )
        )
    except KeyboardInterrupt:
        # 捕获Ctrl+C，执行优雅退出
        signal_handler(signal.SIGINT, None)
    finally:
        running = False
        reset_servos()
        print("程序退出")


if __name__ == "__main__":
    main()
